#ifndef MAP_OPERATION_H
#define MAP_OPERATION_H
#include <ros/ros.h>
#include <pcl/point_types.h>
#include <pcl/common/common.h>
#include "yaml-cpp/yaml.h"
#include "common_lib/topo_map.h"
#include "common_lib/log.h"
#include "common_lib/common.h"
#include <unordered_map>
#include "amr_trajectory_msgs/path_info.h"
#include "amr_trajectory_msgs/point_info.h"
#include "amr_trajectory_msgs/amr_trajectory_msg.h"
#include "localization_msgs/localization_msg.h"

#include "tf/tf.h"

#include <visualization_msgs/Marker.h>
#include <visualization_msgs/MarkerArray.h>

#include "map_operation_msgs/GetPathInfo.h"
#include "map_operation_msgs/GetPointInfo.h"
#include "map_operation_msgs/GetNearestInfo.h"

class MapOperation
{
public:
    MapOperation(ros::NodeHandle nh, std::string map_file, std::string param_file, std::string path);
    ~MapOperation();

    bool Init(void);

    void PublishViewer();

private:
    ros::NodeHandle nh_;
    std::string map_file_;
    std::string param_file_;
    std::string file_path_;
    ros::Publisher pub_vis_map_;
    ros::Publisher pub_pcl_map_;
    
    ros::ServiceServer srv_path_info_;
    ros::ServiceServer srv_point_info_;
    ros::ServiceServer srv_nearest_point_;

    float step_size_ = 0.02f;
    float point_id_dis_precision_ = 0.1f;

    visualization_msgs::MarkerArray msg_vis_map_;

    std::shared_ptr<CommonLib::TopoGraph> ptr_graph_;
    std::vector<amr_trajectory_msgs::path_info> v_path_;
    amr_trajectory_msgs::amr_trajectory_msg all_trajectory_;

    pcl::PointCloud<pcl::PointXYZL>::Ptr ptr_points_;

    common_lib::Common com_;

    bool GetPathInfoService(map_operation_msgs::GetPathInfoRequest &req, map_operation_msgs::GetPathInfoResponse &resp);
    bool GetPointInfoService(map_operation_msgs::GetPointInfoRequest &req, map_operation_msgs::GetPointInfoResponse &resp);
    bool GetNearestInfoService(map_operation_msgs::GetNearestInfoRequest &req, map_operation_msgs::GetNearestInfoResponse &resp);
    
    void AllMapInfo(void);
    void AllTrajectoryInfo(void);
    void CalcHeading(amr_trajectory_msgs::amr_trajectory_msg &all_trajectory);
    amr_trajectory_msgs::amr_trajectory_msg LinePro(amr_trajectory_msgs::path_info &path,    std::string id);
    amr_trajectory_msgs::amr_trajectory_msg CirclePro(amr_trajectory_msgs::path_info &path,  std::string id);
    amr_trajectory_msgs::amr_trajectory_msg BsplinePro(amr_trajectory_msgs::path_info &path, std::string id);

    typedef amr_trajectory_msgs::amr_trajectory_msg (MapOperation::*Fun)(amr_trajectory_msgs::path_info&, std::string );
    std::unordered_map<std::string, Fun> um_path_ = {{"line", &MapOperation::LinePro}, {"circle", &MapOperation::CirclePro}, {"bspline", &MapOperation::BsplinePro}};

    bool RepeatDetection(geometry_msgs::Pose new_point, amr_trajectory_msgs::amr_trajectory_msg &trajectory);

    void GenerateViewer(void);
};

#endif